#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <signal.h>

#include "IPPNS_1034/common_USB_serial.h"

#include <std_msgs/Header.h>
#include <nav_msgs/Odometry.h>

#include <ros/ros.h>


using namespace std;
using namespace IPPNS;




COMMON_USB_SERIAL USB2TTL = COMMON_USB_SERIAL("/dev/ttyUSB4",115200);
int ctrl_flag=1;

void Velcallback(const geometry_msgs::Twist& cmd_vel)
{
	// ROS_INFO("Received a /cmd_vel message!");
	// ROS_INFO("Linear Components:[%f,%f,%f]",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.linear.z);
	// ROS_INFO("Angular Components:[%f,%f,%f]",cmd_vel.angular.x,cmd_vel.angular.y,cmd_vel.angular.z);
  // if (!ctrl_flag){
  //       USB2TTL.Twist2data(cmd_vel,start_code);
  //   ctrl_flag=1;
  // }
}


void OdomCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
 
  ROS_INFO("Linear Components:[%f,%f,%f]",msg->pose.pose.position.x,msg->pose.pose.position.y,msg->pose.pose.position.z);
  usleep(50080);
  if (ctrl_flag){
      std::pair<uint8_t*,int> trans_data=USB2TTL.Pose2data(msg->pose.pose);//输出pair数据类型，用于处理
      USB2TTL.writeData(trans_data.first,trans_data.second);
      ctrl_flag=1;
  }
}





int main(int argc,char **argv)
{

    ros::init(argc, argv, "nav_move_base");
    ros::NodeHandle nh;
    USB2TTL.serialInit();
    
    //订阅pulish point节点得到
    ros::Subscriber odom_sub_=nh.subscribe("/odom",100,OdomCallback);
  	// ros::Subscriber sub = nh.subscribe("/cmd_vel", 1000, Velcallback);    //订阅cmd_vel话题
    // cout<<"fuck"<<endl;
    ros::spin();

    return 0;
}







